#include "lpc.h"

double lpc(const double x[], int N, double ar[], int p) {
    std::vector<double> R;
    assert(N > p);
    R.resize(p+1,0);
    for(int i = 0; i <= p; ++ i) {
        for(int n = 0; n < N-i; ++ n) {
            R[i] += x[n] * x[n+i];
        }
    }

    matrix Rm(p,p);
    for(int i = 0; i < p; ++ i) {
        Rm[i][i] = R[0];
        for(int j = 0; j < i; ++ j) {
            Rm[i][j] = Rm[j][i] = R[i-j];
        }
    }
    matrix Rv(p,1);
    for(int i = 0; i < p; ++ i) {
        Rv[i][0] = R[i+1];
    }

    Rm = Rm.inverse()*Rv;

    for(int i = 0; i < p; ++ i) {
        ar[i] = Rm[i][0];
    }

    double E = R[0];
    for(int i = 1; i <= p; ++ i) {
        E -= ar[i-1] * R[i];
    }
    Rm.Free();
    Rv.Free();
    return sqrt(E);
}

double lpc(const std::vector<double> x, std::vector<double>& ar, int p) {
    int N = x.size();
    assert(N > p);
    ar.resize(p+1, 0);
    std::vector<double> R(p+1,0);
    for(int i = 0; i <= p; ++ i) {
        for(int n = 0; n < N-i; ++ n) {
            R[i] += x[n] * x[n+i];
        }
    }

    matrix Rm(p,p);
    for(int i = 0; i < p; ++ i) {
        Rm[i][i] = R[0];
        for(int j = 0; j < i; ++ j) {
            Rm[i][j] = Rm[j][i] = R[i-j];
        }
    }
    matrix Rv(p,1);
    for(int i = 0; i < p; ++ i) {
        Rv[i][0] = R[i+1];
    }

    Rm = Rm.inverse()*Rv;

    for(int i = 0; i < p; ++ i) {
        ar[i] = Rm[i][0];
    }

    double E = R[0];
    for(int i = 1; i <= p; ++ i) {
        E -= ar[i-1] * R[i];
    }

    Rm.Free();
    Rv.Free();
    return sqrt(E);
}

void lpc_durbin(const std::vector<double> x, std::vector<double>& ar, int p) {
    int N = x.size();
    assert(N > p);
    std::vector<double> R(p+1,0);
    for(int i = 0; i <= p; ++ i) {
        for(int n = 0; n < N-i; ++ n) {
            R[i] += x[n] * x[n+i];
        }
    }
    using std::vector;
    vector<double> K(p,0),E(p,0),C(p,0);
    vector<vector<double>> A(p,vector<double>(p,0));
    K[0] = R[1]/R[0];
    A[0][0] = K[0];
    E[0] = (1-K[0]*K[0])*R[0];

    for(int i = 1; i < p; ++ i) {
        std::fill(C.begin(),C.end(),0);
        double sum = 0;
        for(int j = 0; j <= i-1; ++ j) {
            sum=sum+(A[i-1][j]*R[i-j]);
        }
        C[i] = sum;
        K[i]=(R[i+1]-C[i])/E[i-1];
        A[i][i] = K[i];
        for(int j = 0; j <= i-1; ++ j) {
            A[i][j] = A[i-1][j] - K[i] * A[i-1][i-j-1];
        }
        E[i] = (1-K[i]*K[i])*E[i-1];
    }
    ar.resize(p);
    for(int i = 0; i < p; ++ i) {
        ar[i] = A[p-1][i];
    }
}
